#include "meanshiftDemo.h"

#include "MeanShift.h"
#include "MeanShift.cpp"	// pain from template class

meanshiftDemo::meanshiftDemo()
{
	createDataPoints();

	// debug
	debug_points.clear();
	debug_points.push_back(PointSoup(Qt::yellow));
	debug_points.push_back(PointSoup(Qt::red));
	debug_points.push_back(PointSoup(Qt::green));
	debug_points.push_back(PointSoup(Qt::blue));
}

std::vector<Vec3d> meanshiftDemo::createModeData( int num, Vec3d center )
{
	std::vector<Vec3d> samples;

	double r, theta, phi;
	Vec3d s;
	for (int i = 0; i < num; i++)
	{
		r = pow(uniform_double(), 2);
		theta = uniform_double() * 180;
		phi = uniform_double() * 360;

		s = sphericalToEuclidean(r, theta, phi) + center;
		samples.push_back(s);
	}

	return samples;
}

void meanshiftDemo::createDataPoints()
{
	std::vector<Vec3d> cr = createModeData(1000, Vec3d(-0.5, 0, 0));
	std::vector<Vec3d> cg = createModeData(1000, Vec3d( 0.5, 0, 0));
	std::vector<Vec3d> cb = createModeData(1000, Vec3d( 0, 0, 0.4));

	data = cr;
	data.insert(data.end(), cg.begin(), cg.end());
	data.insert(data.end(), cb.begin(), cb.end());

	meanshift = new MeanShift<3>(data);
}

void meanshiftDemo::draw()
{
	PointSoup data_ps(Qt::cyan, 2.0f);
	foreach (Vec3d p, data) data_ps.addPoint(p);
	data_ps.draw();

	PointSoup models_ps(Qt::red, 20);
	foreach (Vec3d p, models) models_ps.addPoint(p);
	models_ps.draw();


	foreach (PointSoup ps, debug_points) ps.draw();
}

void meanshiftDemo::demo()
{
	meanshift->cluster();
	for (int i = 0; i < (int)debug_points.size(); i++) debug_points[i].clear();

	//foreach (Vec3d p, meanshift->pointsInCluster(0)) debug_points[0].addPoint(p);
	//foreach (Vec3d p, meanshift->pointsInCluster(1)) debug_points[1].addPoint(p);
	//foreach (Vec3d p, meanshift->pointsInCluster(2)) debug_points[2].addPoint(p);
	//foreach (Vec3d p, meanshift->pointsInCluster(3)) debug_points[3].addPoint(p);

	models = meanshift->modes;
}


void meanshiftDemo::demo1()
{
	meanshift->createGrid();
	gridPoints = getGridPoints();


	foreach(Vec3d p, gridPoints)
		debug_points[0].addPoint(p);

	gridID = 0;
}


void meanshiftDemo::stepForward1()
{
	onePassFromPoint(gridPoints[gridID]);

	if (gridID < (int) gridPoints.size() - 1)	gridID++;
}




std::vector<Vec3d> meanshiftDemo::getGridPoints()
{
	std::vector<Vec3d> gps;

	meanshift->createGrid();

	MeanShift<3>::Grid_Coord gcoord(0);
	do {
		gps.push_back(meanshift->gridPoint(gcoord));
	} while (meanshift->moveToNext(gcoord));

	return gps;
}

void meanshiftDemo::onePassFromPoint( Vec3d p )
{
	debug_points[1].addPoint(p);
	debug_points[2].clear();
	std::vector<Vec3d> trajectory;

	Vec3d new_mean = p, old_mean;
	KDResults kd_results;
	do 
	{
		old_mean = new_mean;

		meanshift->kd_tree->ball_search(old_mean, meanshift->h, kd_results);
		if (!kd_results.empty())
		{
			new_mean = meanshift->meanOf(kd_results);
			debug_points[2].addPoint(new_mean);
			trajectory.push_back(new_mean);
		}


	} while ((new_mean - old_mean).norm() > meanshift->e);

	foreach (Vec3d p, trajectory) debug_points[3].addPoint(p);
}

